#if !defined(MTX_ALGORITHM_H)
#define MTX_ALGORITHM_H

#include "mtx/types.h"
#include "mtx/dynamic.h"

#include <vector>
#include <cassert>
#include <cmath>
#include <algorithm>

namespace mtx {

/* 
 * Performs LU decomposition for matrix a (n X n).
 * pvt stores row permutations.
 */

/// returns false if the matrix is singular and decomposition can't be achived
template <class T, class P>
bool LU0(matrix<T> & a, P & pvt) {
	assert(a.rows() == a.cols());

	int i, j, k;

	int n = a.rows();

	for (j = 0; j < n; ++j) {

		for (k = 0; k < j; ++k)
			std::swap(a(k, j), a(pvt[k], j));

		for (k = 0; k < j; ++k)
			for (i = k + 1; i < n; ++i)
				a(i, j) -= a(i, k) * a(k, j);

		int p = j;

		if (j < n - 1) {

			double max = fabs(a(j, j)), f;			
			
			for (k = j + 1; k < n; ++k)
				if ((f = fabs(a(k, j))) > max) {
					max = f;
					p = k;
				}

			pvt[j] = p;

			if (p != j)
				for (k = 0; k <= j; ++k)
					std::swap(a(j, k), a(p, k)); 
		}

		if (a(j, j) == 0)
			return false;

		for (i = j + 1; i < n; ++i)
			a(i, j) /= a(j, j);
	}
	return true;
}

template <class T, class P>
void LU(matrix<T> & a, P & pvt) 
{
	bool res = LU0(a, pvt);
	assert (res);
}

/* 
 * Solves linear system a * x = b for x.
 * a stores the LU decomposition.
 * b stores the solution.
 */
template <class T, class U> 
void sle(matrix<T> & a, vector<U> & b) {

	assert(a.rows() == a.cols());
	assert(a.rows() == b.size());

	size_type n = a.rows();	 

	std::vector<size_type> pivot(n - 1);

	//dynamic::matrix t;
	//t.resize(n, n);

	//assign(t.begin(), t.end(), a.begin());
	LU(a, pivot);

	sle(a, b, pivot);
}

/* 
 * Solves linear system a * x = b for x, 
 * where a has already been LU-decomposed.
 * pvt contains row permutations.
 * b stores the solution.
 */
template <class T, class U, class P>
void sle(const matrix<T> & a, vector<U> & b, P & pvt) {

	assert(a.rows() == a.cols());
	assert(a.rows() == b.size());

	int n = a.rows();

	int i, j;

	for (j = 0; j < n - 1; ++j)
		std::swap(b(j), b(pvt[j]));

	for (i = 0; i < n; ++i)
		for (j = 0; j < i; ++j)
			b(i) -= a(i, j) * b(j);

	for (i = n - 1; i >= 0; --i) {
		for (j = i + 1; j < n; ++j)
			b(i) -= a(i, j) * b(j);
		b(i) /= a(i, i);
	}
}

/* 
 * Inverts a matrix: a := a^-1
 * (current algorithm uses LU-decomposition inside and rather slow)
 */
template <class T> 
void inverse(matrix<T> & a) {
	size_type n = a.rows();	 
	assert (a.cols() == n);

	std::vector<size_type> pivot(n - 1);
	LU(a, pivot);

	std::vector<dynamic::vector> columns(n);
	size_type i, j;
	for (i = 0; i < n; ++i) {
		columns[i].resize(n); //the vector is filled with zeros
		columns[i](i) = 1;
		sle(a, columns[i], pivot);
	}

	for (i = 0; i < n; ++i) {
		for (j = 0; j < n; ++j) {
			a(i, j) = columns[j](i);
		}
	}
}

/* 
 * Performs LU decomposition for matrix a (m X n), m < n,
 * and returns the rank of a.
 * pvt stores row permutations.
 * basis[i] == true for each basis column i. 
 */
template <class T, class P, class B>
size_type LU_(matrix<T> & a, P & pvt, B & basis) {

	const int cc = a.cols(), rr = a.rows();
	assert(rr < cc);

	int i, c, r, k;

	for (c = r = 0; c < cc; ++c) {

		for (k = 0; k < r && k < rr - 1; ++k)
			std::swap(a(k, c), a(pvt[k], c));

		for (k = 0; k < r && k < rr - 1; ++k)
			for (i = k + 1; i < rr; ++i)
				a(i, c) -= a(i, k) * a(k, c);

		basis[c] = false;

		if (r < rr) {

			int p = r;

			double max = fabs(a(r, c));			

			if (r < rr - 1) {
				double f;
				for (k = r + 1; k < rr; ++k)
					if ((f = fabs(a(k, c))) > max) {
						max = f;
						p = k;
					}
				pvt[r] = p;

				if (p != r)
					for (k = 0; k <= c; ++k)
						std::swap(a(r, k), a(p, k)); 
			}

			if (a(r, c) != 0) 
			{
				for (i = r + 1; i < rr; ++i)
					a(i, c) /= a(r, c);
				basis[c] = true;
				r++;
			}
		}
	}

	return size_type(r);
}

// Computes fundamental matrix of system a * x = 0
// where a is m x n, m < n, and stores it in f.
// Returns the number of columns in f.
template <class A, class F>
size_type fundamental(matrix<A> & a, matrix<F> & f) {

	const int rr = a.rows(), cc = a.cols();
	assert(rr < cc);
	assert(f.rows() == cc);

	std::vector<size_type> pivot(a.rows() - 1);
	std::vector<bool> basis(a.cols());

	size_type rank = LU_(a, pivot, basis);

	f = 0.0;

	size_type count = 0;

	int i, j, c, r;

	for (c = 0; c < cc; ++c)
		if (!basis[c]) {
			f(c, count) = 1.0;
			r = rank;
			for (i = cc - 1; i >= 0; --i) {
				if (basis[i]) {
					--r;
					for (j = i + 1; j < cc; ++j)
						f(r, count) -= a(r, j) * f(j, count);
					f(r, count) /= a(r, i);
				}
			}
			count++;
		}

	return count;
}

} // mtx

#endif   // MTX_ALGORITHM_H
